#!/usr/bin/env python3
#
# Does the same thing as the SmartDashboard. Sorta. 
#

import sys
import time
from pynetworktables import *

ip = "10.20.53.2"

NetworkTable.SetIPAddress(ip)
NetworkTable.SetClientMode()
NetworkTable.Initialize()

table = NetworkTable.GetTable("robotMovement")

time.sleep(10)

table.PutBoolean("tableInUse",False)
table.PutBoolean("tableRead",False)

# NetworkTable::GetTable("robotMovement")->PutNumber("angleAdjust",0.0) 
# NetworkTable::GetTable("robotMovement")->PutNumber("currAngle",10.0) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("tableRead",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("tableUpdated",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("tableInUse",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("adjustEnabled",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("foward",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("back",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("left",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("right",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("strafeLeft",false) 
# NetworkTable::GetTable("robotMovement")->PutBoolean("strafeRight",false)
# NetworkTable::GetTable("robotMovement")->PutString("currMode","None") 

table.PutBoolean("tableInUse",True)
print("Setting values for test\n")
table.PutNumber("angle",70.0)
table.PutBoolean("adjustEnabled",True)
table.PutBoolean("foward",True)
table.PutBoolean("back",True)
table.PutBoolean("left",True)
table.PutBoolean("right",True)
table.PutBoolean("strafeLeft",True)
table.PutBoolean("strafeRight",True)
table.PutBoolean("tableUpdated",True)
table.PutBoolean("tableInUse",False)

loop_ctrl = True

inUse = True
time.sleep(6)
print("Entering loop, waiting for robot feedback\n")
while loop_ctrl:
	inUse = table.GetBoolean("tableInUse")
	if(not inUse and table.GetBoolean("tableRead")):
		print("Table was read, now to set all values to stop adjustment\n")
		table.PutBoolean("adjustEnabled",False)
		table.PutBoolean("tableInUse",True)
		table.PutNumber("angle",45)
		table.PutBoolean("foward",False)
		table.PutBoolean("back",False)
		table.PutBoolean("left",False)
		table.PutBoolean("right",False)
		table.PutBoolean("strafeLeft",False)
		table.PutBoolean("strafeRight",False)
		table.PutBoolean("tableRead",False)
		table.PutBoolean("tableUpdated",False)
		table.PutBoolean("tableInUse",False)
		loop_ctrl = False
#    try:
#        print("SmartDashboard::test: %s" % table.GetNumber('X'))
#        num = table.GetNumber('X')
#        num = num + 1
#        table.PutNumber('X',num)
#    except:
#        print("No value yet")
	time.sleep(1)

